package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.Hashtable;


/**
 *
 * @author Fondy Fire
 */
public class AutonomousFF {

    private DigitalInput middleLight;
    private DigitalInput leftLight;
    private DigitalInput rightLight;
    
    private Encoder LeftDriveEncoder;
    private Encoder RightDriveEncoder;
    /*
     OFF_LINE = 0;
    ON_LINE = 2;
    LEFT = 6;
    WAY_LEFT = 4;
    RIGHT = 3; 
    WAY_RIGHT = 1;
    FORK = 5;
    ALL = 7; 
     */
    
    private static final int ON_LINE = 2;
    private static final int OFF_LINE = 0;
    private static final int LEFT = 6;
    private static final int WAY_LEFT = 4;
    private static final int RIGHT = 3; 
    private static final int WAY_RIGHT = 1;
    private static final int FORK = 5;
    private static final int ALL = 7;
    
    private double timeOnLine;
    private RobotArmFF arm;
    private RobotDrive drive;
    private DriveStationFF driveStation;
    private DriverStation dS;
    
    //maps state names to states;
    private Hashtable stateMap;
    private State currState;
    
    /**
     * 500 readings per rotation. WHEELCIRCUM inches per rotation
     */
    private int leftEncoderReadings;
    /**
     * 500 readings per rotation. WHEELCIRCUM inches per rotation
     */
    private int rightEncoderReadings;
    
    /**
     * total inches traveled since the start of autonomous
     */
    private double inchesTraveled;
    
    /** The max motor speed during autonomous */
    private final double MAXSPEED = 0.7;
    /** The time we want to take to get our motors to max speed */
    private final double TIMETOMAX = 2000;
    /** Circumference of wheels, in inches */
    private final double WHEELCIRCUM = 19.5;
    
    private TimerFF timer;
    private double corrCoef;
    private String lastLineReading;


    
    /**
     * Each State has one method that does the actions for that state then 
     * returns the next state. 
     */ private abstract class State{public abstract State getNext(); }
    
     // ======================= Constructor ====================================
    
    public AutonomousFF(RobotDrive drive, RobotArmFF arm, DriveStationFF msgs){
        this.drive = drive;
        this.arm = arm;
        this.driveStation = msgs;
        
        LeftDriveEncoder = new Encoder(3, 4);
        RightDriveEncoder = new Encoder(1, 2);

        rightEncoderReadings = 0;
        leftEncoderReadings = 0;
        
        // Line folowing sensors
        leftLight = new DigitalInput(11);
        middleLight = new DigitalInput(12);
        rightLight = new DigitalInput(13);
        
        stateMap = new Hashtable();
        
        lastLineReading = "none";
        
        timer = new TimerFF();
        
        dS = DriverStation.getInstance();
        //currState = loadTestFSM();
        currState = loadStraightFSM();
//        if(dS != null){
//            if(dS.getDigitalOut(1)){
//                currState = loadStraightFSM();
//            }else if (dS.getDigitalOut(2)) {
//                currState = loadArmTestFSM();
//
//            } else {
//                System.out.println("Driver Station IO all false");
//                //currState = loadTestFSM();
//                currState = loadArmTestFSM();
//            }
//        // dS is null, do safest thing
//        } else {
//           System.out.println("Can't Read Driver Station IO");
//           currState = loadTestFSM();
//        }
        
        
    }
    
    // ======================== FSM Loaders ====================================
    
    private State loadTestFSM(){
        stateMap.put("test1", new State(){

            public State getNext() {
                System.out.println(
                       "L " + LeftDriveEncoder.get() + 
                       " R " + RightDriveEncoder.get()
                       + " Line" + getLineReading()
                       + " Time " + (int) timer.elapsedMilli
                       + " frame Time" + timer.frameMilli);
                return (State) stateMap.get("test2");
            }});

         stateMap.put("test2", new State(){

            private double timeElapsed;
            
            public State getNext() {
                timeElapsed += timer.frameMilli;
                if(timeElapsed > 1000){
                    timeElapsed = 0;
                    return (State) stateMap.get("test1");
                }

                return this;
            }});   
            
        return (State) stateMap.get("test1");
    }

    /**
     * load a state that just moves the arm to the
     * max position.
     * @return
     */
    private State loadArmTestFSM(){
        stateMap.put("armUp", new State(){

            private double timeElapsed;
            
            public State getNext() {
                timeElapsed += timer.frameMilli;
                arm.postUpAuto();
                arm.armToTop(0.7);
                return this;
                
            }});
        return (State) stateMap.get("armUp");    
    }

    
    /** Load the default FMS */
    private State loadStraightFSM(){
        
        stateMap.put("wait", new State(){

            private double timeElapsed;
            public State getNext() {
                timeElapsed += timer.frameMilli;
                if ( timeElapsed > 3000){
                    return (State) stateMap.get("forward");
                }
                return this;
            }
            
        });
        
        stateMap.put("forward", new State(){

            private double timeElapsed;
            private double offLineTime = 0;
            

            public State getNext() {
                timeElapsed += timer.frameMilli;
                arm.postUpAuto();
                arm.armToTop(0.7);
                
                if(inchesTraveled < 180){
                    //drive.tankDrive(speedUp(timeElapsed), speedUp(timeElapsed));
                    if(getLineReading() == OFF_LINE){
                        offLineTime += timer.frameMilli;
                    } else {
                       offLineTime = 0;
                    }
                    if(offLineTime < 500){
                        drive.drive(speedUp(timeElapsed), heading(getLineReading()));
                    } else {
                        return (State) stateMap.get("hang");
                    }
                    
                    return this;
                } else {
                    return (State) stateMap.get("hang");
                }
                
            }});

         stateMap.put("hang", new State(){

            private double timeElapsed;
            
            public State getNext() {
                timeElapsed += timer.frameMilli;
                arm.stopArmAuto();
                arm.postDownAuto();
                
                if(timeElapsed < 500){
                    arm.tubeRotateForwardAuto();
                } else {
                    arm.tubePushOutAuto();
                }
                
                
                
                if(timeElapsed > 3000){
                    return (State) stateMap.get("backup");
                }
                return this;

            }});
            
       stateMap.put("backup", new State(){

            private double timeElapsed;
            
            public State getNext() {
                timeElapsed += timer.frameMilli;
                if(timeElapsed < 3000){
                    drive.tankDrive(-speedUp(timeElapsed), -speedUp(timeElapsed));
                   
                } 
                 return this;
            }});
       return (State) stateMap.get("wait");
    }
    
    /**
     * Loads the state machine for starting in the fork position;
     * fork defaults to going left, not finished
     */
    private State loadForkFSM(){

        // The starting state. drive to the fork
        stateMap.put("lineToFork", new State(){
            private double stateTime = 0;
            public State getNext() {
                stateTime += timer.frameMilli;
                int lightState = getLineReading();

                if (hasLine(lightState)){
                    drive.drive(speedUp(stateTime), heading(lightState));
                } else if (lightState == FORK || lightState == ALL) {
                    return (State) stateMap.get("atFork");
                }
                return this;
        }});
        // =

        stateMap.put("atFork", new State(){
            private double stateTime = 0;
            public State getNext() {

            stateTime += timer.frameMilli;
            drive.tankDrive(-0.5, 0.5);
            // Get encoder readings, use these to determine theta 
            return this;
       }});


            
       return (State) stateMap.get("lineToFork");
    }
      // ========== Public methods ====================================
    
    public void init(){
        LeftDriveEncoder.reset();
        RightDriveEncoder.reset();
        LeftDriveEncoder.start();
        RightDriveEncoder.start();
        AutoSpeed = 0;
        AutoStep = 1;
        StepCounter = 0;
        arm.initAuto();
        timer.init();

        
    }
    
    
    
    public void update(){
        arm.updateAuto();
        timer.update();
        updateEncoders();
//        System.out.println("Inches travled: " + inchesTraveled);
//        System.out.println( LeftDriveEncoder.get());
//        System.out.println( RightDriveEncoder.get());
        currState = currState.getNext();
        
    }
    
    // ========== Helper Methods ======================================
    
    
    
    
    /**
     * Soft start for autonomous give Milliseconds
     * 
     * Tested, verified.
     * @return
     */
    private double speedUp(double t) {
        if (t > 2000){return MAXSPEED;} else { return (MAXSPEED/TIMETOMAX) * t;}
    }

    /** returns true if the given reading corresponds to an aquired line */
    private boolean hasLine(int reading){
         return (reading == ON_LINE  
                || reading == LEFT
                || reading == WAY_LEFT 
                || reading == RIGHT  
                || reading == WAY_RIGHT);
    }
    
    /**
     * Gives a heading designed to keep us on the line. Linked to the
     * encoder readings.
     *
     * @param mode
     * @return
     */
    private double heading(int lightReading){
        //if(lightReading == ON_LINE){
            //timeOnLine += timer.frameMilli;
            // TODO: This value pulled out of crazyspace, probably will not
            // work. NOTE: These lines are not used
            //double timerCoef = 0.001;
            //corrCoef = 1 / (timerCoef * (timeOnLine + 1));
            //return 0;
        //} else {
         //   timeOnLine = 0;

            //if(lightReading == LEFT || lightReading == WAY_LEFT){
                //return (-0.7);
            //}

            //if (lightReading == RIGHT || lightReading == WAY_RIGHT){
                //return (0.7);
            //}

        if (!leftLight.get()) {
            lastLineReading = "!left";
            return -0.7;
            
            
        }
        
         if (!rightLight.get()) {
            lastLineReading = "!right";
            return 0.7;
        }
        
        return 0;
        //}
        
    }
    
    /**
     * set readings 
     * set inches traveled
     */
    private void updateEncoders(){
        leftEncoderReadings = LeftDriveEncoder.get();
        rightEncoderReadings = RightDriveEncoder.get();
        inchesTraveled = rightEncoderReadings * (WHEELCIRCUM / 500);
    }
    
    private int getLineReading(){

        // This rather odd system is used to avoid enumerating the states with
        // a big if statement. (In this method at least)
        // light bits == LMR

        int out = 0;
        out += ((rightLight.get()) ? 1: 0);
        out += (((middleLight.get()) ? 1: 0) << 1);
        out += (((leftLight.get()) ? 1: 0) << 2);

        return out;
    }
    
  
    
    
    /* ========================= Obsolete Methods =========================== */
    
    /** used  as a fail safe */
    private int frames = 0;
    private double AutoSpeed; // drive speed in autonomous
    private int AutoStep;  // autonomous steps
    private int StepCounter;
    private int onLineCount = 0;
    private double correctCoef = .01;
    private boolean printOnce = true;
    
    private void LineTracer() {
        if (!leftLight.get()) {
            drive.drive(AutoSpeed, -0.7);
            if (printOnce) {
                System.out.println("!left: " + onLineCount);
                onLineCount = 0;
                printOnce = false;
            }
        } else if (!rightLight.get()) {
            drive.drive(AutoSpeed, 0.7);
            if (printOnce) {
                System.out.println("!right: " + onLineCount);
                onLineCount = 0;
                printOnce = false;
            }
        } else if (!middleLight.get()) {
            drive.drive(AutoSpeed, 0.0);
            onLineCount++;
            printOnce = true;
        } // partners.
        // fail safe. If we go off the the line, we will stop, rather than bump our
        // partners.
        if (!leftLight.get() && !middleLight.get() && !rightLight.get()) {
            frames++;
            if (frames > 10) {
                drive.drive(0.0, 0.0);
            }
        } else {
            frames = 0;
        }
        //if(Math.abs(LeftDriveEncoder.get()) <= 1000){
        //    robotDrive.drive(0.25, 0);
        //}
        //if(Math.abs(LeftDriveEncoder.get()) <= 1000){
        //    robotDrive.drive(0.25, 0);
        //}
    }
}
